/*
 * MainThread.cpp
 *
 *  Created on: 04.12.2012
 *      Author: Michael
 */

#include "MainThread.h"

#include "../hardware.h"
#include "../Kalman/Kalman.h"
#include "../PIDController/AnglePID.h"
#include "../PIDController/VelocityPID.h"

#include "../SMControl/AngleSMC.h"
#include "../SMControl/VelocitySMC.h"

extern Kalman kalman;
extern Systemstate state;

extern Gyroscope gyro;
extern Magnetometer mag;
extern Camera cam;

extern AnglePID anglePID;
extern VelocityPID velPID;
extern AngleSMC angleSMC;
extern VelocitySMC velSMC;

MainThread::MainThread(const char* name, const long prio) :
		Thread(name, prio) {
}

MainThread::~MainThread() {
	// TODO Auto-generated destructor stub
}

void MainThread::init() {
}

void MainThread::run() {

	setPeriodicBeat(2100 * MILLISECONDS, SAMPLETIME_MS * MILLISECONDS);
	suspendUntilNextBeat();
	state.frameReceived = false;
	state.shiftIsActive = true;
	state.notTaken = true;
//	state.desAngle = 0;
	float oldAngleKal = 0;

	state.angleMag = mag.getOrientation();
	state.velGyro = gyro.getScaledAngularRateZ();
	state.angleKalman = kalman.getAngle();
	state.angleGyro = state.angleKalman;
	while (1) {
		GREEN_ON;
		// UPDATE state VALUES
		// first update measured values
		state.angleMag = mag.getOrientation();
		state.velGyro = gyro.getScaledAngularRateZ();
		// update calculated values
		state.angleGyro -= SAMPLETIME_S * (state.velGyro - state.gyroBias);

		if(state.angleGyro > 360){
			state.angleGyro -= 360;
		} else if(state.angleGyro < 0) {
			state.angleGyro += 360;
		}


		state.angleKalman = kalman.getAngle();
		state.velMag = (oldAngleKal - state.angleKalman) / SAMPLETIME_S;
		oldAngleKal = state.angleKalman;
		// last update kalman values
//		state.velKalman; TODO
		state.temperature = gyro.getTemp();
		// PID STUFF
		if (state.PID_Angle) {
			anglePID.run();
			if (state.notTaken) {
				if (state.angleKalman - WINDOW <= state.desAngle
						&& state.angleKalman + WINDOW >= state.desAngle) {
//					cam.Capture();
					state.notTaken = false;
				}
			}
		} else if (state.PID_Vel) {
			velPID.run();
		} else if (state.SMC_Vel){
			velSMC.run();
		} else if (state.SMC_Angle){
			angleSMC.run();
			if (state.notTaken) {
				if (state.angleKalman - WINDOW <= state.desAngle
						&& state.angleKalman + WINDOW >= state.desAngle) {
//					cam.Capture();
					state.notTaken = false;
				}
			}
		}
		GREEN_OFF;
		suspendUntilNextBeat();
	}
}
